#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <uORB/uORB.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_platform_common/posix.h>
#include <stdio.h>
#include <unistd.h>

extern "C" __EXPORT int test_joystick_buttons_main(int argc, char *argv[]);

int test_joystick_buttons_main(int argc, char *argv[])
{
    int manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
    px4_pollfd_struct_t fds[] = {
        { .fd = manual_control_sub, .events = POLLIN }
    };

    printf("Waiting for joystick button and stick events...\n");

    while (true) {
        int poll_ret = px4_poll(fds, 1, 1000); // 1s timeout

        if (poll_ret > 0 && (fds[0].revents & POLLIN)) {
            struct manual_control_setpoint_s manual_control{};
            orb_copy(ORB_ID(manual_control_setpoint), manual_control_sub, &manual_control);

            printf("Joystick buttons bitmask: 0x%08x\n", manual_control.buttons);

            // 打印摇杆和辅助通道的值
            printf("roll: %.2f\tpitch: %.2f\tyaw: %.2f\tthrottle: %.2f\n", (double)manual_control.roll, (double)manual_control.pitch, (double)manual_control.yaw, (double)manual_control.throttle);
            printf("flaps: %.2f\taux1: %.2f\taux2: %.2f\taux3: %.2f\taux4: %.2f\taux5: %.2f\taux6: %.2f\n",
                (double)manual_control.flaps, (double)manual_control.aux1, (double)manual_control.aux2, (double)manual_control.aux3, (double)manual_control.aux4, (double)manual_control.aux5, (double)manual_control.aux6);

            // 打印每个按钮的状态
            for (int i = 0; i < 16; ++i) {
                printf("Button %d: %s\n", i+1, (manual_control.buttons & (1 << i)) ? "Pressed" : "Released");
            }
            printf("\n");
        }
    }

    orb_unsubscribe(manual_control_sub);
    return 0;
}
